﻿using System;
using System.Collections.Generic;
using Allegro.Framework;
using Allegro.MathInterface;

namespace Allegro.Mathlib
{
    /// <summary>
    /// Ridder's diff. method, cf. Numerical Recipes  5.7
    /// </summary>
    public class RiddersDifferentiationMethod : IDifferentiation
    {
        private const double CON = 1.4; // Stepsize is decreased by CON at each iteration
        private const double CON2 = CON * CON;
        private const double BIG = 1.0e30;
        private const int NTAB = 10; // Sets max. size of tableau
        private const double SAFE = 2.0; // Return when error is SAFE worse than the best so far

        private double[,] _a;

        /// <summary>
        /// Initial step size
        /// </summary>
        public double StepSize { get; set; }

        /// <summary>
        /// Error after differentiation
        /// </summary>
        public double Error;

        /// <summary>
        /// Function to be differentiated
        /// </summary>
        public Function Func { get; set; }

        public RiddersDifferentiationMethod()
        {
            StepSize = double.NaN;
            Func = null;
            _a = new double[NTAB, NTAB];
        }
        public RiddersDifferentiationMethod(Function f, double initialStepSize)
        {
            StepSize = initialStepSize;
            Func = f;
            _a = new double[NTAB, NTAB];
        }

        public double Df(double x)
        {
            if (StepSize.Equals(double.NaN))
            {
                ErrorHandler.Error("RiddersDifferentiationMethod.Df: StepSize has not been set");
            }
            if (Func == null)
            {
                ErrorHandler.Error("RiddersDifferentiationMethod.Df: Function has not been set");
            }
            double h = StepSize;
            int i, j;
            double errt, fac, hh, ans = double.NaN;

            if (h == 0.0)
            {
                ErrorHandler.Error("RiddersDifferentiationMethod.Df: step must be nonzero");
            }
            hh = h;
            _a[0, 0] = (Func(x + hh) - Func(x - hh)) / (2.0 * hh);
            Error = BIG;
            for (i = 1; i < NTAB; i++)
            {
                // Successive columns in the Neville tableau will go to smaller stepsizes and higher orders of extrapolation
                hh /= CON;
                _a[0, i] = (Func(x + hh) - Func(x - hh)) / (2.0 * hh); // Try new, smaller stepsize
                fac = CON2;
                for (j = 1; j <= i; j++)
                {
                    // Compute extrapolations of various orders, requiring no new function evaluations
                    _a[j, i] = (_a[j - 1, i] * fac - _a[j - 1, i - 1]) / (fac - 1.0);
                    fac *= CON2;
                    errt = Math.Max(Math.Abs(_a[j, i] - _a[j - 1, i]), Math.Abs(_a[j, i] - _a[j - 1, i - 1]));
                    // The error strategy is to compare each new extrapolation to one order lower, both at the present stepsize and the previous one
                    if (errt <= Error)
                    {
                        // If error is decreased, save the improved answer
                        Error = errt;
                        ans = _a[j, i];
                    }
                }
                // If higher order is worse by significant factor SAFE, then quit early
                if (Math.Abs(_a[i, i] - _a[i - 1, i - 1]) >= SAFE * Error)
                    break;
            }
            return ans;
        }
    }

    /// <summary>
    /// Gradient computation using Ridders numerical diff. method
    /// </summary>
    public class GradientUsingRiddersMethod : Gradient
    {
        public GradientUsingRiddersMethod(VectorFunction f, double[] stepSizes)
            : base(f, new RiddersDifferentiationMethod(), stepSizes)
        {
        }
    }
}
